/*
 * gjk.cpp
 *
 *  Created on: Feb 27, 2012
 *      Author: Michael Ziminsky
 */

using arma::cross;
using arma::dot;

typedef std::vector<vector3> Simplex;

namespace
{

vector3 support(const ConvexShape &a, const ConvexShape &b, const vector3 &d)
{
    return a.getSupport(d) - b.getSupport(-d);
}

vector3 cross_aba(const vector3 &a, const vector3 &b)
{
    return cross(cross(a, b), a);
}

bool update(Simplex &s, vector3 &v)
{
    bool found = false;
    if (s.size() == 3)
    {
        enum POINT
        {
            C = 0, B, A
        };
        vector3 ao = -s[A];

        vector3 ab = s[B] - s[A];

        vector3 ac = s[C] - s[A];
        vector3 abc = cross(ab, ac);
        if (dot(cross(ab, abc), ao) > 0)
        {
            s.erase(s.begin());
            v = cross_aba(ab, ao);
        }
        else if (dot(cross(abc, ac), ao) > 0)
        {
            s.erase(++s.begin());
            v = cross_aba(ac, ao);
        }
        else found = true;
    }
    return found;
}

struct Edge
{
    scalar distance;
    vector3 normal;
    int pos;
};

inline Edge findClosestEdge(const Simplex &s)
{
    Edge closest;
    closest.distance = std::numeric_limits<scalar>::max();
    for (uint i = 0, j = 1; i < s.size(); ++i, ++j)
    {
        if (j == s.size()) j = 0;
        const vector3 &a = s[i], &b = s[j];
        vector3 e = b - a;
        vector3 n = cross_aba(e, a);
        normalize(n);
        scalar d = dot(n, a);
        if (d < closest.distance)
        {
            closest.distance = d;
            closest.normal = n;
            closest.pos = j;
        }
    }
    return closest;
}

void getEPAContactInfo(const ConvexShape &a, const ConvexShape &b, Simplex &s, vector3 &normal, scalar &depth)
{
    for (int i = 0; i < 20; ++i)
    {
        Edge e = findClosestEdge(s);
        vector3 p = support(a, b, e.normal);
        scalar d = dot(p, e.normal);
        if (d - e.distance < .00001)
        {
            normal = e.normal;
            depth = d;
            break;
        }
        else s.insert(s.begin() + e.pos, p);
    }
}

} // namespace

bool intersect(const ConvexShape &a, const ConvexShape &b, vector3 &normal, scalar &depth, vector3 v)
{
    Simplex simplex;

    simplex.push_back(support(a, b, v));
    v = -simplex.back();
    simplex.push_back(support(a, b, v));

    if (dot(simplex.back(), v) < 0) return false;

    v = cross_aba(simplex.front() - simplex.back(), -simplex.back());

    for (unsigned char i = 0; i < 20; ++i)
    {
        simplex.push_back(support(a, b, v));
        if (dot(simplex.back(), v) < 0) return false;
        else if (update(simplex, v)) break;
    }
    getEPAContactInfo(a, b, simplex, normal, depth);
    return true;
}
